/**
 * @inproceedings{cvpr2013Fast_Match,
 *	  title={Fast-Match: Fast Affine Template Matching},
 *	  author={Korman, Simon and Reichman, Daniel and Tsur, Gilad and Avidan, Shai},
 *	  booktitle={Computer Vision and Pattern Recognition (CVPR), 2013 IEEE Conference on},
 *	  pages={1940--1947},
 *	  year={2013},
 *	  organization={IEEE}
 * }
 */

#pragma once
#ifndef TM_GPU_FAST_MATCH_H_
#define TM_GPU_FAST_MATCH_H_

#include <cuda_runtime.h>
#include <helper_cuda.h>
#include <cv.h>

namespace recognition {
namespace gpu {

/**
 * Implements Fast-Match: Fast Affine Template Matching by Korman, Simon and
 * Reichman, Daniel and Tsur, Gilad and Avidan, Shai // CVPR 2013.
 *
 * Contains both GPU and CPU implementations. Simple usage:
 *    Mat image_	= cv::imread("image.png", CV_LOAD_IMAGE_GRAYSCALE);
 *    Mat template_ = cv::imread("template.png", CV_LOAD_IMAGE_GRAYSCALE);
 *    ... // make odd dimensions and smooth image, template
 *
 *    recognition::gpu::FAsT_Match fast_match = recognition::gpu::FAsT_Match::Builder()
 *		.I1(template_)
 *		.I2(image_)
 *		.ScaleRange(0.5, 2.0)
 *		.Delta(0.25)
 *		.Epsilon(0.15)
 *		.PhotometricInvariance(true)
 *		.build();
 *
 *    fast_match.RunGpu();
 *    // fast_match.RunCpu();
 */
class FAsT_Match {
public:
	class Builder : public IBuilder<FAsT_Match> {
	public:
		Builder& I1(cv::Mat& image) {
			this->I1_ = image;
			return *this;
		}

		Builder& I2(cv::Mat& image) {
			this->I2_ = image;
			return *this;
		}

		Builder& ScaleRange(double min_scale, double max_scale) {
			this->min_scale_ = min_scale;
			this->max_scale_ = max_scale;
			return *this;
		}

		Builder& Delta(double delta) {
			this->delta_ = delta;
			return *this;
		}

		Builder& Epsilon(double epsilon) {
			this->epsilon_ = epsilon;
			return *this;
		}

		Builder& PhotometricInvariance(bool use_photometric) {
			this->use_photometric_ = use_photometric;
			return *this;
		}

		FAsT_Match build() {
			return FAsT_Match(this);
		}

	private:
		cv::Mat I1_, I2_;
		double min_scale_, max_scale_;
		double epsilon_, delta_;
		bool use_photometric_;

		friend class FAsT_Match;
	};

	// Defines creation method of net S
	enum ExtensionType { RANDOM_EXTENSION, FULL_EXTENSION };

	static const log4cxx::LoggerPtr logger;
	static const log4cxx::LoggerPtr output;

	~FAsT_Match() {
		set_d_configs(NULL);
		set_d_affines(NULL);
		set_d_filter(NULL);
		set_d_Q(NULL);
		set_d_S(NULL);

		set_h_distances(NULL);
	}

	// Runs FAsT-Match on GPU
	FastMatchResult RunGpu();
	// Runs FAsT-Match on CPU
	FastMatchResult RunCpu();
	// Evaluates number of steps in each DOF for further construction of configs.
	void GetConfigDims(uint& ntx_steps, uint& nty_steps, uint& nr_steps, uint& ns_steps);

	// Creates Mat(nconfigs, 6). Each row describes one affine transformation.
	// 		config[0] - tx
	// 		config[1] - ty
	// 		config[2] - r2
	// 		config[3] - sx
	// 		config[4] - sy
	// 		config[5] - r1
	//
	// \param configs	- matrix of configs
	void CreateConfigs(cv::Mat& configs);
	// Creates nconfigs * 6 array of type double of configs. It's a row wise
	// representation of 2d array[nconfigs][6], where each row describes one
	// affine transformation. The created array is stored on device memory
	// and might be accessed by \ref ::d_configs().
	//
	// \param nconfigs	- number of configs
	void _cu_CreateConfigs(uint& nconfigs);

	// Creates Mat(naffines, 6). Each row describes one affine transformation.
	// Six elements of one affine represent two rows of matrix of affine
	// transformation:
	// 		[affine[0] = a11	;	affine[1] = a12		;	affine[2] = a13]
	// 		[affine[3] = a21	;	affine[4] = a22		;	affine[5] = a23]
	//
	// \param configs	- configs to be translated into \p affines
	// \param affines	- affine transformations
	// \param filter	- indicates whether corresponding transformation lies
	// within bounds
	// \param naffines	- number of affine transformations within bounds
	void ConfigsToAffines(cv::Mat& configs, cv::Mat& affines, bool* filter, uint& naffines);
	// Creates nconfigs * 6 array of type double of affine transformations.
	// It's a row wise representation of 2d array[naffines][6], where each
	// row describes one affine transformation. The created array is stored
	// on device memory and might be accessed by \ref ::d_affines(). Also
	// creates bool array of size nconfigs to indicate whether corresponding
	// affine transformation lies within bounds. The created array is stored
	// on device memory and might be accessed by \ref ::d_filter().
	//
	// \param nconfigs	- number of configs to be translated into affines
	void _cu_ConfigsToAffines(const uint nconfigs);

	// Creates \p dst matrix from \p src by applying \p filter with size of
	// \p src.rows. \p dst elements correspond to true elements of \p filter.
	//
	// \param src		- source matrix
	// \param dst		- destination matrix
	// \param filter	- enumerates \p src elements to be copied into \p dst
	void FilterMatrix(cv::Mat& src, cv::Mat& dst, bool* filter);
	// Creates arrays of configs and corresponding affine transformations on
	// device memory of size \p naffines * 6. Resulting configs might be
	// accessed by \ref ::d_configs(). Resulting affine transformations might
	// be accessed by \ref ::d_affines(). For each config (and affine) it
	// applies already prepared filter, indicating whether resulting config
	// (and affine) is copied to the destination array. It is stored on
	// device memory and might be accessed by \ref ::d_filter().
	//
	// \param nconfigs	- number of configs
	// \param naffines	- resulting number of filtered affine transformations
	// and configs corresponding to them
	void _cu_FilterConfigsAndAffines(const uint nconfigs, uint& naffines);

	// Evaluates SAD of affine transformations in \p affines_.
	//
	// \param affines_	- Mat(naffines, 6) of affine transformations, where each
	// row describes one transformation
	// \param xs_	- x coordinates of template's points in which SAD is evaluated
	// \param ys_	- y coordinates of template's points in which SAD is evaluated
	// \param distances_	- Mat(1, naffines) of SAD errors, where each elements
	// corresponds to one affine transformation, described by row in \p affines
	void EvaluateAffines(cv::Mat& affines_, std::vector<int>& xs_, std::vector<int>& ys_, cv::Mat& distances_);
	// Evaluates SAD of affine transformations in array stored on host, which
	// might be accessed by \ref ::h_distances(). It has size of naffines and each
	// element corresponds to one affine transformation, stored on device memory
	// and might be accessed by \ref ::d_affines().
	//
	// \param naffines	- number of affine transformations
	// \param xs	- x coordinates of template's points in which SAD is evaluated
	// \param ys	- y coordinates of template's points in which SAD is evaluated
	void _cu_EvaluateAffines(uint naffines, std::vector<int>& xs, std::vector<int>& ys);

	// Creates matrix of good configs Q(nQ, 6). Each row describes one config, with
	// SAD close to \p best_distance.
	//
	// \param configs	- all configs
	// \param best_distance - best SAD error amonng \p distances
	// \param new_delta	- precision parameter delta on current step
	// \param distances	- SAD errors of configs
	// \param Q			- matrix of good configs Q
	// \param percentage_higher_limit - indicates if nQ/nconfig ratio is too high
	void Create_Q(cv::Mat& configs, double best_distance, double new_delta, cv::Mat& distances, cv::Mat& Q, bool& percentage_higher_limit);
	// Creates nQ * 6 array of doubles on device memory, where each 6 elements
	// represent one good config of constructed set Q. The array might be accessed
	// by \ref ::d_Q(). It copies configs from \ref ::d_configs(), which have SAD
	// SAD error close to \p best_distance.
	//
	// \param best_distance - best SAD error amonng \p distances
	// \param new_delta	- precision parameter delta on current step
	// \param naffines	- number of affine transformations
	// \param nQ		- number of good configs (size of set Q)
	// \param percentage_higher_limit - indicates if nQ/nconfig ratio is too high
	void _cu_Create_Q(double best_distance, double new_delta, uint naffines, uint& nQ, bool& percentage_higher_limit);

	// Creates set \p S. It takes \p configs and repeats it \p extension_scale
	// times. Then it randomly disturbs elements of expanded set by small steps.
	//
	// \param configs	- configs of affine transformations
	// \param level		- current iteration of algorithm
	// \param extension_scale	- how many times to repeat \p configs
	// \param delta_fact		- factor of delta change
	// \param S			- output set S
	void Create_S_random(cv::Mat& configs, uint level, uint extension_scale, double delta_fact, cv::Mat& S);
	// Creates set \p S. It takes \p configs and repeats it \p 3^6 times. Then
	// disturbs elements of expanded set by small steps, therefore generates
	// the complete extension of \p configs in defined range.
	//
	// \param configs	- configs of affine transformations
	// \param level		- current iteration of algorithm
	// \param delta_fact		- factor of delta change
	// \param S			- output set S
	void Create_S_full(cv::Mat& configs, uint level, double delta_fact, cv::Mat& S);
	// Creates double array of extension_scale * nQ * 6 elements on device, which
	// might be accessed by \ref ::d_S(). Each 6 elements represent one affine
	// transformation in set S. It takes set Q on device by \ref ::d_Q() and repeats
	// it \p extension_scale times. Then it randomly disturbs elements of expanded
	// set by small steps.
	//
	// \param nQ	- size of set Q
	// \param level - current iteration of algorithm
	// \p extension_scale	- how many times to repeat \p configs
	// \p delta_fact		- factor of delta change
	// \p full_extension	- fully extent Q or not
	void _cu_Create_S(uint nQ, uint level, uint extension_scale, double delta_fact, bool full_extension);

	// Creates augmented matrix of affine transformation from \p config.
	//
	// \param config	- config, representing one affine transformation
	void CreateAffineTransformation(double* config, double affine[3][3]);

//	void EvaluateConfigs2(int n_transformations, int n_points, double* pattern, double* image, double* cover, int w2, double c, int* x, int* y, double* correlations);

	// Fields accessors
	cv::Mat& I1() { return I1_; }
	cv::Mat& I2() { return I2_; }
	FastMatchBoundaries bounds() const { return bounds_; }

private:
	FAsT_Match(Builder* builder) {
		this->I1_ = builder->I1_;
		this->I2_ = builder->I2_;
		this->min_scale_ = builder->min_scale_;
		this->max_scale_ = builder->max_scale_;
		this->epsilon_ = builder->epsilon_;
		this->delta_ = builder->delta_;
		this->use_photometric_ = builder->use_photometric_;

		make_images_continuous();
		init_host_pointers();
		init_device_pointers();
		init();
		eval_bounds();
	}

	void init() {
		h1_ = I1_.rows; w1_ = I1_.cols;
		h2_ = I2_.rows; w2_ = I2_.cols;
		r1x_ = (w1_ - 1) / 2; r1y_ = (h1_ - 1) / 2;
		r2x_ = (w2_ - 1) / 2; r2y_ = (h2_ - 1) / 2;
	}

	void eval_bounds() {
		bounds_.tx = BoundsWithStep<double>(delta_ * w1_ / sqrt(2), -(r2x_ - r1x_ * min_scale_), r2x_ - r1x_ * min_scale_);
		bounds_.ty = BoundsWithStep<double>(delta_ * h1_ / sqrt(2), -(r2y_ - r1y_ * min_scale_), r2y_ - r1y_ * min_scale_);
		bounds_.r = BoundsWithStep<double>(delta_ * sqrt(2), -M_PI, M_PI);
		bounds_.s = BoundsWithStep<double>(delta_ / sqrt(2), min_scale_, max_scale_);
	}

	void make_images_continuous();

	double GetDevMemoryUsedMb() {
		return recognition::GetDevMemoryUsedMb();
	}

	void ReportDevMemoryUsage(const char* func_name, double dev_mem_used_start) {
		recognition::ReportDevMemoryUsage(logger, func_name, dev_mem_used_start);
	}

	void ReportMethodExecutionTime(const char* func_name, double time) {
		recognition::ReportMethodExecutionTime(logger, func_name, time);
	}

	void ReportKernelExecutionTime(const char* func_name, double time) {
		recognition::ReportKernelExecutionTime(logger, func_name, time);
	}

	double* d_configs() { return d_configs_; }
	void set_d_configs(double* val) {
		if (d_configs_ != NULL)
			checkCudaErrors( cudaFree(d_configs_) );
		d_configs_ = val;
	}
	double* d_affines() { return d_affines_; }
	void set_d_affines(double* val) {
		if (d_affines_ != NULL)
			checkCudaErrors( cudaFree(d_affines_) );
		d_affines_ = val;
	}
	bool* d_filter() { return d_filter_; }
	void set_d_filter(bool* val) {
		if (d_filter_ != NULL)
			checkCudaErrors( cudaFree(d_filter_) );
		d_filter_ = val;
	}
	double* d_Q() { return d_Q_; }
	void set_d_Q(double* val) {
		if (d_Q_ != NULL)
			checkCudaErrors( cudaFree(d_Q_) );
		d_Q_ = val;
	}
	double* d_S() { return d_S_; }
	void set_d_S(double* val) {
		if (d_S_ != NULL)
			checkCudaErrors( cudaFree(d_S_) );
		d_S_ = val;
	}

	double* h_distances() { return h_distances_; }
	void set_h_distances(double* val) {
		if (h_distances_ != NULL)
			checkCudaErrors( cudaFreeHost(h_distances_) );
		h_distances_ = val;
	}

	void init_host_pointers() {
		h_distances_		= NULL;
	}

	void init_device_pointers() {
		d_configs_			= NULL;
		d_affines_			= NULL;
		d_filter_			= NULL;
		d_Q_				= NULL;
		d_S_				= NULL;
	}

	uint GetNPoints() {
		return round(10 / (epsilon_ * epsilon_));
	}

	double GetThreshPerDelta(double delta);

	cv::Mat I1_, I2_;
	double min_scale_, max_scale_;
	FastMatchBoundaries bounds_;
	double epsilon_, delta_;
	bool use_photometric_;

	int h1_, w1_, h2_, w2_;
	int r1x_, r1y_, r2x_, r2y_;

	double* h_distances_;

	double* d_configs_;
	double* d_affines_;
	bool*	d_filter_;
	double*	d_Q_;
	double* d_S_;
};

}	// namespace gpu
}	// namespace recognition

#endif	// TM_GPU_FAST_MATCH_H_
